#pragma once
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <string>
#include <functional>
#include <memory>

using namespace std::chrono_literals;

template<typename ServiceT>
class ServiceCaller {
public:
    struct Result {
        bool success;
        std::string message;
        typename ServiceT::Response::SharedPtr response;

        Result(bool s, const std::string& msg, typename ServiceT::Response::SharedPtr resp)
            : success(s), message(msg), response(resp) {}
    };

    using Callback = std::function<void(const Result&)>;

    static void CallAsync(
        rclcpp::Node::SharedPtr node,
        typename rclcpp::Client<ServiceT>::SharedPtr client,
        typename ServiceT::Request::SharedPtr request,
        Callback callback,
        int retry_count = 0,
        std::chrono::seconds timeout = 3s)
    {
        if (!client->wait_for_service(1s)) {
            callback(Result{false, "Service unavailable", nullptr});
            return;
        }

        auto future = client->async_send_request(request);
        auto shared_future = std::make_shared<decltype(future)>(std::move(future));

        auto timer = node->create_wall_timer(
            timeout,
            [node, client, request, callback, retry_count, shared_future]() {
                if (shared_future->wait_for(0s) != std::future_status::ready) {
                    RCLCPP_WARN(node->get_logger(), "Timeout, retry: %d", retry_count);
                    if (retry_count < 3) {
                        CallAsync(node, client, request, callback, retry_count+1);
                    } else {
                        callback(Result{false, "Max retries", nullptr});
                    }
                }
            });

        std::thread([node, shared_future, callback, timer](){
            shared_future->wait();
            timer->cancel();
            
            try {
                auto response = shared_future->get();
                callback(Result{response->success, response->message, response});
            } catch (const std::exception& e) {
                callback(Result{false, std::string("Exception: ")+e.what(), nullptr});
            }
        }).detach();
    }
};